/*
 * lib.h
 *
 *  Created on: 2010.06.27
 *      Author: Tadas
 */

#ifndef LIB_H_
#define LIB_H_

#include <cmath>

#define EPS 1e-6
#define JMAX 16

using namespace std;

const int lygSkaic = 2;
const int lygsk = 2;

double **allocate2d(int eil, int stl)
{
	double **mas;

	mas = new double *[eil];
	assert(mas!=NULL);

	for (int i=0; i<eil; i++)
		{
			mas[i] = new double [stl];
			assert(mas[i]!=NULL);
		}

	return mas;
}

double * allocate1d(int imax)
{
	double * mas;
	int i;

	mas = new double[imax];
	assert (mas != NULL);
	for (i=0 ; i<imax ; i++) mas[i] = 0;

	return mas;
}

void deallocate2d(double **mas, int eil)
{
	for (int i=0; i<eil; i++)
		delete [] mas[i];

	delete [] mas;
}

void deallocate1d (double * mas){	
	delete [] mas;	
}

void step(double *y, double t, double h, void(*func)(double, double *, double *))
{
	int i;
	double th, tt, t6;
	double dym[lygsk], dyt[lygsk], yt[lygsk], f[lygsk];

	tt = 0.5*h;
	t6 = h/6;
	th = t+tt;

	func(t, y, f);

	for (i=0; i<lygsk; i++) yt[i] = y[i]+tt*f[i];

	func(th, yt, dyt);

	for (i=0; i<lygsk; i++) yt[i] = y[i]+tt*dyt[i];

	func(th, yt, dym);

	for (i=0; i<lygsk; i++)
	{
		yt[i] = y[i] + h*dym[i];
		dym[i] += dyt[i];
	}

	func(t+h, yt, dyt);

	for(i=0; i<lygsk; i++) y[i] += t6* (f[i]+2*dym[i]+dyt[i]);
}
//---------------------------------------------------------------------------

void RK4solver(double **y, double *x, int ipoint, void(*func)(double, double *, double *))
{
	double yNew[lygsk], yOld[lygsk], yt[lygsk];
	double hh, h, temp;
	int i, j, k, flag, in;

	for (i=1; i<ipoint; i++)
	{
		h=x[i]-x[i-1];

		for (j=0; j<JMAX; j++)
		{
			for (k=0; k<lygsk; k++) yt[k]=y[i-1][k];

			if (j==0)
			{
				step(yt, x[i-1], h, func);
				for (k=0; k<lygsk; k++) yOld[k] = yt[k];
			}
			else
			{
				for (k=0, in=1; k<j; k++) in<<=1;
				hh=h/double(in);
				for (k=0; k<in; k++) step(yt, x[i-1]+k*hh, hh, func);
				for (k=0; k<lygsk; k++) yNew[k]=yt[k];
				flag =0;

				for (k=0; k<lygsk; k++)
				{
					if (fabs(yOld[k]-yNew[k])>EPS)
					{
						flag = 1;
						break;
					}
				}

				if (flag == 0)
				{
					for (k=0; k<lygsk; k++) y[i][k] = yNew[k];
					break;
				}

				for (k=0; k<lygsk; k++) yOld[k] = yNew[k];
			}
		}
		if (j==JMAX)
		{
			printf("Per daug zingsniu RK4solver\n");
			for (k=0; k<lygsk; k++) y[i][k] = yNew[k];
		}
	}
}



#endif /* LIB_H_ */
